/**
 ******************************************************************************
 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */





#include "vl53l1_ll_def.h"
#include "vl53l1_ll_device.h"
#include "vl53l1_platform.h"
#include "vl53l1_platform_ipp.h"
#include "vl53l1_register_map.h"
#include "vl53l1_register_funcs.h"
#include "vl53l1_register_settings.h"
#include "vl53l1_hist_map.h"
#include "vl53l1_hist_structs.h"
#include "vl53l1_core.h"
#include "vl53l1_wait.h"
#include "vl53l1_api_preset_modes.h"
#include "vl53l1_silicon_core.h"
#include "vl53l1_api_core.h"
#include "vl53l1_api_calibration.h"

#ifdef VL53L1_LOG_ENABLE
  #include "vl53l1_api_debug.h"
#endif


#define LOG_FUNCTION_START(fmt, ...) \
	_LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
#define LOG_FUNCTION_END(status, ...) \
	_LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
	_LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, status, \
		fmt, ##__VA_ARGS__)

#define trace_print(level, ...) \
	_LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \
	level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)


VL53L1_Error VL53L1_run_ref_spad_char(
	VL53L1_DEV        Dev,
	VL53L1_Error     *pcal_status)
{


	VL53L1_Error status = VL53L1_ERROR_NONE;
	VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);

	uint8_t comms_buffer[6];

	VL53L1_refspadchar_config_t *prefspadchar  = &(pdev->refspadchar);

	LOG_FUNCTION_START("");



	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_enable_powerforce(Dev);



	if (status == VL53L1_ERROR_NONE)
		status =
		VL53L1_set_ref_spad_char_config(
			Dev,
			prefspadchar->VL53L1_p_009,
			prefspadchar->timeout_us,
			prefspadchar->target_count_rate_mcps,
			prefspadchar->max_count_rate_limit_mcps,
			prefspadchar->min_count_rate_limit_mcps,
			pdev->stat_nvm.osc_measured__fast_osc__frequency);



	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_run_device_test(
					Dev,
					prefspadchar->device_test_mode);



	if (status == VL53L1_ERROR_NONE)
		status =
		VL53L1_ReadMulti(
			Dev,
			VL53L1_REF_SPAD_CHAR_RESULT__NUM_ACTUAL_REF_SPADS,
			comms_buffer,
			2);

	if (status == VL53L1_ERROR_NONE) {
		pdev->dbg_results.ref_spad_char_result__num_actual_ref_spads =
				comms_buffer[0];
		pdev->dbg_results.ref_spad_char_result__ref_location =
				comms_buffer[1];
	}



	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_WriteMulti(
				Dev,
				VL53L1_REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS,
				comms_buffer,
				2);

	if (status == VL53L1_ERROR_NONE) {
		pdev->customer.ref_spad_man__num_requested_ref_spads =
				comms_buffer[0];
		pdev->customer.ref_spad_man__ref_location =
				comms_buffer[1];
	}



	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_ReadMulti(
				Dev,
				VL53L1_RESULT__SPARE_0_SD1,
				comms_buffer,
				6);



	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_WriteMulti(
				Dev,
				VL53L1_GLOBAL_CONFIG__SPAD_ENABLES_REF_0,
				comms_buffer,
				6);

	if (status == VL53L1_ERROR_NONE) {
		pdev->customer.global_config__spad_enables_ref_0 =
				comms_buffer[0];
		pdev->customer.global_config__spad_enables_ref_1 =
				comms_buffer[1];
		pdev->customer.global_config__spad_enables_ref_2 =
				comms_buffer[2];
		pdev->customer.global_config__spad_enables_ref_3 =
				comms_buffer[3];
		pdev->customer.global_config__spad_enables_ref_4 =
				comms_buffer[4];
		pdev->customer.global_config__spad_enables_ref_5 =
				comms_buffer[5];
	}

#ifdef VL53L1_LOG_ENABLE

	if (status == VL53L1_ERROR_NONE)
		VL53L1_print_customer_nvm_managed(
			&(pdev->customer),
			"run_ref_spad_char():pdev->lldata.customer.",
			VL53L1_TRACE_MODULE_REF_SPAD_CHAR);
#endif

	if (status == VL53L1_ERROR_NONE) {

		switch (pdev->sys_results.result__range_status) {

		case VL53L1_DEVICEERROR_REFSPADCHARNOTENOUGHDPADS:
			status = VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS;
			break;

		case VL53L1_DEVICEERROR_REFSPADCHARMORETHANTARGET:
			status = VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH;
			break;

		case VL53L1_DEVICEERROR_REFSPADCHARLESSTHANTARGET:
			status = VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW;
			break;
		}
	}



	*pcal_status = status;



	IGNORE_STATUS(
		IGNORE_REF_SPAD_CHAR_NOT_ENOUGH_SPADS,
		VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS,
		status);

	IGNORE_STATUS(
		IGNORE_REF_SPAD_CHAR_RATE_TOO_HIGH,
		VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH,
		status);

	IGNORE_STATUS(
		IGNORE_REF_SPAD_CHAR_RATE_TOO_LOW,
		VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW,
		status);


	LOG_FUNCTION_END(status);

	return status;
}


VL53L1_Error VL53L1_run_xtalk_extraction(
	VL53L1_DEV	                        Dev,
	VL53L1_Error                       *pcal_status)
{


	VL53L1_Error status        = VL53L1_ERROR_NONE;
	VL53L1_LLDriverData_t *pdev =
		VL53L1DevStructGetLLDriverHandle(Dev);



	VL53L1_xtalkextract_config_t *pX = &(pdev->xtalk_extract_cfg);
	VL53L1_xtalk_config_t *pC = &(pdev->xtalk_cfg);
	VL53L1_xtalk_calibration_results_t *pXC = &(pdev->xtalk_cal);

	uint8_t results_invalid  = 0;

	uint8_t i                = 0;
	uint16_t tmp16 = 0;

	uint8_t measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK;

	LOG_FUNCTION_START("");





	VL53L1_init_histogram_bin_data_struct(
			0,
			(uint16_t)VL53L1_HISTOGRAM_BUFFER_SIZE,
			&(pdev->xtalk_results.central_histogram_avg));

	VL53L1_init_histogram_bin_data_struct(
			0,
			(uint16_t)VL53L1_HISTOGRAM_BUFFER_SIZE,
			&(pdev->xtalk_results.central_histogram_sum));



	if (status == VL53L1_ERROR_NONE)
		status =
		VL53L1_set_preset_mode(
		Dev,
		VL53L1_DEVICEPRESETMODE_HISTOGRAM_XTALK_PLANAR,

		pX->dss_config__target_total_rate_mcps,
		pX->phasecal_config_timeout_us,
		pX->mm_config_timeout_us,
		pX->range_config_timeout_us,

		100);



	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_disable_xtalk_compensation(Dev);



	pdev->xtalk_results.max_results    = VL53L1_MAX_XTALK_RANGE_RESULTS;
	pdev->xtalk_results.active_results = pdev->zone_cfg.active_zones+1;



	pdev->xtalk_results.central_histogram__window_start = 0xFF;
	pdev->xtalk_results.central_histogram__window_end   = 0x00;

	pdev->xtalk_results.num_of_samples_status = 0x00;
	pdev->xtalk_results.zero_samples_status   = 0x00;
	pdev->xtalk_results.max_sigma_status      = 0x00;

	for (i = 0; i < pdev->xtalk_results.max_results; i++) {
		pdev->xtalk_results.VL53L1_p_002[i].no_of_samples           = 0;
		pdev->xtalk_results.VL53L1_p_002[i].signal_total_events_avg = 0;
		pdev->xtalk_results.VL53L1_p_002[i].signal_total_events_sum = 0;
		pdev->xtalk_results.VL53L1_p_002[i].rate_per_spad_kcps_sum  = 0;
		pdev->xtalk_results.VL53L1_p_002[i].rate_per_spad_kcps_avg  = 0;
		pdev->xtalk_results.VL53L1_p_002[i].sigma_mm_sum            = 0;
		pdev->xtalk_results.VL53L1_p_002[i].sigma_mm_avg            = 0;

		pdev->xtalk_results.VL53L1_p_002[i].median_phase_sum        = 0;
		pdev->xtalk_results.VL53L1_p_002[i].median_phase_avg        = 0;

	}


	if (status == VL53L1_ERROR_NONE) {

		status =
		VL53L1_get_and_avg_xtalk_samples(
		Dev,

		pX->num_of_samples,

		measurement_mode,

		pX->algo__crosstalk_extract_max_valid_range_mm,
		pX->algo__crosstalk_extract_min_valid_range_mm,
		pX->algo__crosstalk_extract_max_valid_rate_kcps,

		0x0,
		0x4,
		&(pdev->xtalk_results),
		&(pdev->xtalk_results.central_histogram_sum),
		&(pdev->xtalk_results.central_histogram_avg));
	}







	if (status == VL53L1_ERROR_NONE)
		if ((pdev->xtalk_results.VL53L1_p_002[4].no_of_samples == 0) ||
			(pdev->xtalk_results.VL53L1_p_002[4].sigma_mm_avg >
			((uint32_t)pX->algo__crosstalk_extract_max_sigma_mm
					<< 5)))
			results_invalid = 0x01;



#ifdef VL53L1_LOG_ENABLE
	if (status == VL53L1_ERROR_NONE)
		VL53L1_print_xtalk_range_results(
			&(pdev->xtalk_results),
			"pdev->xtalk_results",
			VL53L1_TRACE_MODULE_CORE);
#endif

	if ((status == VL53L1_ERROR_NONE) && (results_invalid == 0)) {

		status =
			VL53L1_ipp_xtalk_calibration_process_data(
					Dev,
					&(pdev->xtalk_results),
					&(pdev->xtalk_shapes),
					&(pdev->xtalk_cal));

	}
	if ((status == VL53L1_ERROR_NONE) && (results_invalid == 0)) {
		for (i = 0; i < VL53L1_BIN_REC_SIZE; i++)
			pXC->algo__xtalk_cpo_HistoMerge_kcps[i] =
			pXC->algo__crosstalk_compensation_plane_offset_kcps;
		pC->algo__crosstalk_compensation_x_plane_gradient_kcps =
		pXC->algo__crosstalk_compensation_x_plane_gradient_kcps;
		pC->algo__crosstalk_compensation_y_plane_gradient_kcps =
		pXC->algo__crosstalk_compensation_y_plane_gradient_kcps;
		pC->algo__crosstalk_compensation_plane_offset_kcps =
		pXC->algo__crosstalk_compensation_plane_offset_kcps;
	}



	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_enable_xtalk_compensation(Dev);




	if (status == VL53L1_ERROR_NONE) {

		for (i = 0; i < pdev->xtalk_results.max_results; i++) {

			if (pdev->xtalk_results.VL53L1_p_002[i].no_of_samples !=

				pX->num_of_samples) {

				pdev->xtalk_results.num_of_samples_status =
				pdev->xtalk_results.num_of_samples_status |
					(1 << i);
			}

			if (pdev->xtalk_results.VL53L1_p_002[i].no_of_samples ==
				0x00) {
				pdev->xtalk_results.zero_samples_status =
				pdev->xtalk_results.zero_samples_status |
					(1 << i);
			}




			tmp16 = pX->algo__crosstalk_extract_max_sigma_mm;
			if (pdev->xtalk_results.VL53L1_p_002[i].sigma_mm_avg >
				((uint32_t)tmp16 << 5)) {
				pdev->xtalk_results.max_sigma_status =
					pdev->xtalk_results.max_sigma_status |
					(1 << i);
			}

		}
	}


	if (results_invalid > 0) {

		if (pdev->xtalk_results.VL53L1_p_002[4].no_of_samples == 0) {
			status = VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL;
		} else {


			if (pdev->xtalk_results.VL53L1_p_002[4].sigma_mm_avg >
			(((uint32_t)pX->algo__crosstalk_extract_max_sigma_mm)
				<< 5)) {
				status =
				VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL;
			}

		}
	} else {

		if (pdev->xtalk_results.zero_samples_status != 0x00) {
			status = VL53L1_WARNING_XTALK_NO_SAMPLES_FOR_GRADIENT;
		} else {
			if (pdev->xtalk_results.max_sigma_status != 0x00) {
				status =
				VL53L1_WARNING_XTALK_SIGMA_LIMIT_FOR_GRADIENT;
			} else {
				if (pdev->xtalk_results.num_of_samples_status !=
						0x00)
					status =
					VL53L1_WARNING_XTALK_MISSING_SAMPLES;
			}
		}
	}



	pdev->xtalk_results.cal_status = status;
	*pcal_status = pdev->xtalk_results.cal_status;



	IGNORE_STATUS(
		IGNORE_XTALK_EXTRACTION_NO_SAMPLE_FAIL,
		VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL,
		status);

	IGNORE_STATUS(
		IGNORE_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL,
		VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL,
		status);

	IGNORE_STATUS(
		IGNORE_XTALK_EXTRACTION_NO_SAMPLE_FOR_GRADIENT_WARN,
		VL53L1_WARNING_XTALK_NO_SAMPLES_FOR_GRADIENT,
		status);

	IGNORE_STATUS(
		IGNORE_XTALK_EXTRACTION_SIGMA_LIMIT_FOR_GRADIENT_WARN,
		VL53L1_WARNING_XTALK_SIGMA_LIMIT_FOR_GRADIENT,
		status);

	IGNORE_STATUS(
		IGNORE_XTALK_EXTRACTION_MISSING_SAMPLES_WARN,
		VL53L1_WARNING_XTALK_MISSING_SAMPLES,
		status);

#ifdef VL53L1_LOG_ENABLE



	VL53L1_print_customer_nvm_managed(
		&(pdev->customer),
		"run_xtalk_extraction():pdev->lldata.customer.",
		VL53L1_TRACE_MODULE_XTALK_DATA);

	VL53L1_print_xtalk_config(
		&(pdev->xtalk_cfg),
		"run_xtalk_extraction():pdev->lldata.xtalk_cfg.",
		VL53L1_TRACE_MODULE_XTALK_DATA);

	VL53L1_print_xtalk_extract_config(
		&(pdev->xtalk_extract_cfg),
		"run_xtalk_extraction():pdev->lldata.xtalk_extract_cfg.",
		VL53L1_TRACE_MODULE_XTALK_DATA);

	VL53L1_print_histogram_bin_data(
		&(pdev->hist_data),
		"run_xtalk_extraction():pdev->lldata.hist_data.",
		VL53L1_TRACE_MODULE_XTALK_DATA);

	VL53L1_print_xtalk_histogram_data(
		&(pdev->xtalk_shapes),
		"pdev->lldata.xtalk_shapes.",
		VL53L1_TRACE_MODULE_XTALK_DATA);

	VL53L1_print_xtalk_range_results(
		&(pdev->xtalk_results),
		"run_xtalk_extraction():pdev->lldata.xtalk_results.",
		VL53L1_TRACE_MODULE_XTALK_DATA);

#endif

	LOG_FUNCTION_END(status);

	return status;

}



VL53L1_Error VL53L1_get_and_avg_xtalk_samples(
		VL53L1_DEV	                  Dev,
		uint8_t                       num_of_samples,
		uint8_t                       measurement_mode,
		int16_t                       xtalk_filter_thresh_max_mm,
		int16_t                       xtalk_filter_thresh_min_mm,
		uint16_t                      xtalk_max_valid_rate_kcps,
		uint8_t                       xtalk_result_id,
		uint8_t                       xtalk_histo_id,
		VL53L1_xtalk_range_results_t *pXR,
		VL53L1_histogram_bin_data_t  *psum_histo,
		VL53L1_histogram_bin_data_t  *pavg_histo)
{



	VL53L1_Error status        = VL53L1_ERROR_NONE;
	VL53L1_LLDriverData_t *pdev =
		VL53L1DevStructGetLLDriverHandle(Dev);

#ifdef VL53L1_LOG_ENABLE
	VL53L1_LLDriverResults_t *pres =
		VL53L1DevStructGetLLResultsHandle(Dev);
#endif

	VL53L1_range_results_t      *prs =
			(VL53L1_range_results_t *) pdev->wArea1;

	VL53L1_range_data_t         *prange_data;
	VL53L1_xtalk_range_data_t   *pxtalk_range_data;

	uint8_t i                = 0;
	uint8_t j                = 0;
	uint8_t zone_id          = 0;
	uint8_t final_zone       = pdev->zone_cfg.active_zones+1;
	uint8_t valid_result;

	uint8_t smudge_corr_en   = 0;




	smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;

	status = VL53L1_dynamic_xtalk_correction_disable(Dev);


	VL53L1_load_patch(Dev);



	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_init_and_start_range(
				Dev,
				measurement_mode,
				VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS);


	for (i = 0; i <= (final_zone*num_of_samples); i++) {



		if (status == VL53L1_ERROR_NONE)
			status = VL53L1_wait_for_range_completion(Dev);



		if (status == VL53L1_ERROR_NONE)
			status =
				VL53L1_get_device_results(
					Dev,
					VL53L1_DEVICERESULTSLEVEL_FULL,
					prs);



		if (status == VL53L1_ERROR_NONE &&
			pdev->ll_state.rd_device_state !=
			VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC) {

			zone_id = pdev->ll_state.rd_zone_id + xtalk_result_id;
			prange_data       = &(prs->VL53L1_p_002[0]);


			if (prs->active_results > 1) {
				for (j = 1;
				j < prs->active_results; j++) {
					if (prs->VL53L1_p_002[j].median_range_mm
						<
						prange_data->median_range_mm)
						prange_data =
						&(prs->VL53L1_p_002[j]);

				}
			}

			pxtalk_range_data = &(pXR->VL53L1_p_002[zone_id]);



			if ((prs->active_results > 0) &&
				(prange_data->median_range_mm <
						xtalk_filter_thresh_max_mm) &&
				(prange_data->median_range_mm >
						xtalk_filter_thresh_min_mm) &&
				(prange_data->VL53L1_p_012 <
				(uint32_t)(xtalk_max_valid_rate_kcps * 16)))
				valid_result = 1;
			else
				valid_result = 0;

			if (valid_result == 1) {

				pxtalk_range_data->no_of_samples++;

				pxtalk_range_data->rate_per_spad_kcps_sum +=
					prange_data->VL53L1_p_012;

				pxtalk_range_data->signal_total_events_sum +=
					prange_data->VL53L1_p_013;

				pxtalk_range_data->sigma_mm_sum +=
					(uint32_t)prange_data->VL53L1_p_005;



				pxtalk_range_data->median_phase_sum +=
					(uint32_t)prange_data->VL53L1_p_014;




			}

			if ((valid_result == 1) && (zone_id >= 4)) {
				status = VL53L1_sum_histogram_data(
						&(pdev->hist_data),
						psum_histo);



				if (prange_data->VL53L1_p_015 <
					pXR->central_histogram__window_start)
					pXR->central_histogram__window_start =
					prange_data->VL53L1_p_015;


				if (prange_data->VL53L1_p_016 >
					pXR->central_histogram__window_end)
					pXR->central_histogram__window_end =
						prange_data->VL53L1_p_016;

			}

		}



#ifdef VL53L1_LOG_ENABLE
		if (status == VL53L1_ERROR_NONE) {
			VL53L1_print_range_results(
					&(pres->range_results),
					"pres->range_results.",
					VL53L1_TRACE_MODULE_CORE);
		}
#endif



		if (status == VL53L1_ERROR_NONE)
			status = VL53L1_wait_for_firmware_ready(Dev);



		if (status == VL53L1_ERROR_NONE)
			status =
				VL53L1_clear_interrupt_and_enable_next_range(
					Dev,
					measurement_mode);


	}




	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_stop_range(Dev);

	VL53L1_unload_patch(Dev);



	for (i = 0; i < (pdev->zone_cfg.active_zones+1); i++) {

		pxtalk_range_data = &(pXR->VL53L1_p_002[i+xtalk_result_id]);

		if (pxtalk_range_data->no_of_samples > 0) {
			pxtalk_range_data->rate_per_spad_kcps_avg =
			pxtalk_range_data->rate_per_spad_kcps_sum /
			(uint32_t)pxtalk_range_data->no_of_samples;

			pxtalk_range_data->signal_total_events_avg =
			pxtalk_range_data->signal_total_events_sum /
			(int32_t)pxtalk_range_data->no_of_samples;

			pxtalk_range_data->sigma_mm_avg =
			pxtalk_range_data->sigma_mm_sum /
			(uint32_t)pxtalk_range_data->no_of_samples;



			pxtalk_range_data->median_phase_avg =
				pxtalk_range_data->median_phase_sum /
				(uint32_t)pxtalk_range_data->no_of_samples;



		} else {
			pxtalk_range_data->rate_per_spad_kcps_avg =
				pxtalk_range_data->rate_per_spad_kcps_sum;
			pxtalk_range_data->signal_total_events_avg =
				pxtalk_range_data->signal_total_events_sum;
			pxtalk_range_data->sigma_mm_avg =
				pxtalk_range_data->sigma_mm_sum;



			pxtalk_range_data->median_phase_avg =
					pxtalk_range_data->median_phase_sum;


		}
	}



	memcpy(pavg_histo, &(pdev->hist_data),
			sizeof(VL53L1_histogram_bin_data_t));



	if (status == VL53L1_ERROR_NONE) {

		pxtalk_range_data = &(pXR->VL53L1_p_002[xtalk_histo_id]);

		status = VL53L1_avg_histogram_data(
			pxtalk_range_data->no_of_samples,
			psum_histo,
			pavg_histo);
	}




	if (status == VL53L1_ERROR_NONE) {
		if (smudge_corr_en == 1)
			status = VL53L1_dynamic_xtalk_correction_enable(Dev);
	}


	LOG_FUNCTION_END(status);

	return status;

}



VL53L1_Error VL53L1_run_offset_calibration(
	VL53L1_DEV	                  Dev,
	int16_t                       cal_distance_mm,
	uint16_t                      cal_reflectance_pc,
	VL53L1_Error                 *pcal_status)
{


	VL53L1_Error status        = VL53L1_ERROR_NONE;
	VL53L1_LLDriverData_t *pdev =
		VL53L1DevStructGetLLDriverHandle(Dev);

	VL53L1_DevicePresetModes device_preset_modes[
				VL53L1_MAX_OFFSET_RANGE_RESULTS];

	VL53L1_range_results_t     *prange_results =
			(VL53L1_range_results_t *) pdev->wArea1;

	VL53L1_range_data_t        *pRData = NULL;
	VL53L1_offset_range_data_t *pfs     = NULL;
	VL53L1_general_config_t *pG = &(pdev->gen_cfg);
	VL53L1_additional_offset_cal_data_t *pAO = &(pdev->add_off_cal_data);

	uint8_t  i                      = 0;
	uint8_t  m                      = 0;
	uint8_t  measurement_mode       =
		VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK;
	uint16_t manual_effective_spads =
		pG->dss_config__manual_effective_spads_select;

	uint8_t num_of_samples[VL53L1_MAX_OFFSET_RANGE_RESULTS];

	uint8_t smudge_corr_en   = 0;

	LOG_FUNCTION_START("");



	switch (pdev->offset_calibration_mode) {

	case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__HISTOGRAM:
	case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__HISTOGRAM_PRE_RANGE_ONLY:
		device_preset_modes[0] =
			VL53L1_DEVICEPRESETMODE_HISTOGRAM_RANGING;
		device_preset_modes[1] =
			VL53L1_DEVICEPRESETMODE_HISTOGRAM_RANGING_MM1_CAL;
		device_preset_modes[2] =
			VL53L1_DEVICEPRESETMODE_HISTOGRAM_RANGING_MM2_CAL;
	break;

	default:
		device_preset_modes[0] =
			VL53L1_DEVICEPRESETMODE_STANDARD_RANGING;
		device_preset_modes[1] =
			VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM1_CAL;
		device_preset_modes[2] =
			VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM2_CAL;
	break;
	}



	num_of_samples[0] = pdev->offsetcal_cfg.pre_num_of_samples;
	num_of_samples[1] = pdev->offsetcal_cfg.mm1_num_of_samples;
	num_of_samples[2] = pdev->offsetcal_cfg.mm2_num_of_samples;




	switch (pdev->offset_calibration_mode) {

	case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY:
	case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__HISTOGRAM_PRE_RANGE_ONLY:

		pdev->offset_results.active_results  = 1;

	break;

	default:

		pdev->customer.mm_config__inner_offset_mm  = 0;
		pdev->customer.mm_config__outer_offset_mm  = 0;
		pdev->offset_results.active_results  =
			VL53L1_MAX_OFFSET_RANGE_RESULTS;

	break;
	}

	pdev->customer.algo__part_to_part_range_offset_mm = 0;



	pdev->offset_results.max_results   = VL53L1_MAX_OFFSET_RANGE_RESULTS;
	pdev->offset_results.cal_distance_mm       = cal_distance_mm;
	pdev->offset_results.cal_reflectance_pc    = cal_reflectance_pc;

	for (m = 0; m <  VL53L1_MAX_OFFSET_RANGE_RESULTS; m++) {

		pfs = &(pdev->offset_results.VL53L1_p_002[m]);
		pfs->preset_mode         = 0;
		pfs->no_of_samples       = 0;
		pfs->effective_spads     = 0;
		pfs->peak_rate_mcps      = 0;
		pfs->VL53L1_p_005            = 0;
		pfs->median_range_mm     = 0;
	}




	smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;

	status = VL53L1_dynamic_xtalk_correction_disable(Dev);



	for (m = 0; m < pdev->offset_results.active_results; m++) {

		pfs = &(pdev->offset_results.VL53L1_p_002[m]);

		pfs->preset_mode         = device_preset_modes[m];



		if (status == VL53L1_ERROR_NONE)
			status =
			VL53L1_set_preset_mode(
			Dev,
			device_preset_modes[m],

			pdev->offsetcal_cfg.dss_config__target_total_rate_mcps,
			pdev->offsetcal_cfg.phasecal_config_timeout_us,
			pdev->offsetcal_cfg.mm_config_timeout_us,
			pdev->offsetcal_cfg.range_config_timeout_us,

			100);

		pG->dss_config__manual_effective_spads_select =
				manual_effective_spads;


		VL53L1_load_patch(Dev);

		if (status == VL53L1_ERROR_NONE)
			status =
			VL53L1_init_and_start_range(
				Dev,
				measurement_mode,
				VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS);

		for (i = 0; i <= (num_of_samples[m]+2); i++) {



			if (status == VL53L1_ERROR_NONE)
				status =
					VL53L1_wait_for_range_completion(Dev);



			if (status == VL53L1_ERROR_NONE)
				status =
					VL53L1_get_device_results(
						Dev,
						VL53L1_DEVICERESULTSLEVEL_FULL,
						prange_results);



			pRData  = &(prange_results->VL53L1_p_002[0]);

			if ((prange_results->active_results > 0 &&
				prange_results->stream_count   > 1) &&
				(pRData->range_status ==
				VL53L1_DEVICEERROR_RANGECOMPLETE)) {

				pfs->no_of_samples++;
				pfs->effective_spads +=
				(uint32_t)pRData->VL53L1_p_006;
				pfs->peak_rate_mcps  +=
				(uint32_t)pRData->peak_signal_count_rate_mcps;
				pfs->VL53L1_p_005        +=
					(uint32_t)pRData->VL53L1_p_005;
				pfs->median_range_mm +=
					(int32_t)pRData->median_range_mm;

				pfs->dss_config__roi_mode_control =
				pG->dss_config__roi_mode_control;
				pfs->dss_config__manual_effective_spads_select =
				pG->dss_config__manual_effective_spads_select;

			}



			if (status == VL53L1_ERROR_NONE)
				status =
					VL53L1_wait_for_firmware_ready(Dev);



			if (status == VL53L1_ERROR_NONE)
				status =
				VL53L1_clear_interrupt_and_enable_next_range(
					Dev,
					measurement_mode);
		}



		if (status == VL53L1_ERROR_NONE)
			status = VL53L1_stop_range(Dev);



		if (status == VL53L1_ERROR_NONE)
			status = VL53L1_WaitUs(Dev, 1000);
		VL53L1_unload_patch(Dev);


		if (pfs->no_of_samples > 0) {

			pfs->effective_spads += (pfs->no_of_samples/2);
			pfs->effective_spads /= pfs->no_of_samples;

			pfs->peak_rate_mcps  += (pfs->no_of_samples/2);
			pfs->peak_rate_mcps  /= pfs->no_of_samples;

			pfs->VL53L1_p_005        += (pfs->no_of_samples/2);
			pfs->VL53L1_p_005        /= pfs->no_of_samples;

			pfs->median_range_mm += (pfs->no_of_samples/2);
			pfs->median_range_mm /= pfs->no_of_samples;

			pfs->range_mm_offset  =  (int32_t)cal_distance_mm;
			pfs->range_mm_offset -= pfs->median_range_mm;


			if (pfs->preset_mode ==
				VL53L1_DEVICEPRESETMODE_STANDARD_RANGING)
				manual_effective_spads =
					(uint16_t)pfs->effective_spads;
		}
	}



	switch (pdev->offset_calibration_mode) {

	case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY:
	case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__HISTOGRAM_PRE_RANGE_ONLY:


		pdev->customer.mm_config__inner_offset_mm +=
		(int16_t)pdev->offset_results.VL53L1_p_002[0].range_mm_offset;
		pdev->customer.mm_config__outer_offset_mm +=
		(int16_t)pdev->offset_results.VL53L1_p_002[0].range_mm_offset;
	break;

	default:

		pdev->customer.mm_config__inner_offset_mm =
		(int16_t)pdev->offset_results.VL53L1_p_002[1].range_mm_offset;
		pdev->customer.mm_config__outer_offset_mm =
		(int16_t)pdev->offset_results.VL53L1_p_002[2].range_mm_offset;
		pdev->customer.algo__part_to_part_range_offset_mm = 0;



		pAO->result__mm_inner_actual_effective_spads =
		(uint16_t)pdev->offset_results.VL53L1_p_002[1].effective_spads;
		pAO->result__mm_outer_actual_effective_spads =
		(uint16_t)pdev->offset_results.VL53L1_p_002[2].effective_spads;

		pAO->result__mm_inner_peak_signal_count_rtn_mcps =
		(uint16_t)pdev->offset_results.VL53L1_p_002[1].peak_rate_mcps;
		pAO->result__mm_outer_peak_signal_count_rtn_mcps =
		(uint16_t)pdev->offset_results.VL53L1_p_002[2].peak_rate_mcps;

		break;
	}



	pdev->cust_dmax_cal.ref__actual_effective_spads =
		(uint16_t)pdev->offset_results.VL53L1_p_002[0].effective_spads;
	pdev->cust_dmax_cal.ref__peak_signal_count_rate_mcps =
		(uint16_t)pdev->offset_results.VL53L1_p_002[0].peak_rate_mcps;


	pdev->cust_dmax_cal.ref__distance_mm = cal_distance_mm * 16;

	pdev->cust_dmax_cal.ref_reflectance_pc = cal_reflectance_pc;
	pdev->cust_dmax_cal.coverglass_transmission = 0x0100;



	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_set_customer_nvm_managed(
				Dev,
				&(pdev->customer));




	if (status == VL53L1_ERROR_NONE) {
		if (smudge_corr_en == 1)
			status = VL53L1_dynamic_xtalk_correction_enable(Dev);
	}




	for (m = 0; m < pdev->offset_results.active_results; m++) {

		pfs = &(pdev->offset_results.VL53L1_p_002[m]);

		if (status == VL53L1_ERROR_NONE) {

			pdev->offset_results.cal_report = m;

			if (pfs->no_of_samples < num_of_samples[m])
				status =
				VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES;


			if (m == 0 && pfs->VL53L1_p_005 >
				((uint32_t)VL53L1_OFFSET_CAL_MAX_SIGMA_MM << 5))
				status =
				VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;

			if (pfs->peak_rate_mcps >
				VL53L1_OFFSET_CAL_MAX_PRE_PEAK_RATE_MCPS)
				status =
				VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH;

			if (pfs->dss_config__manual_effective_spads_select <
				VL53L1_OFFSET_CAL_MIN_EFFECTIVE_SPADS)
				status =
				VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW;

			if (pfs->dss_config__manual_effective_spads_select == 0)
				status =
				VL53L1_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL;

			if (pfs->no_of_samples == 0)
				status = VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
		}
	}



	pdev->offset_results.cal_status = status;
	*pcal_status = pdev->offset_results.cal_status;



	IGNORE_STATUS(
		IGNORE_OFFSET_CAL_MISSING_SAMPLES,
		VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES,
		status);

	IGNORE_STATUS(
		IGNORE_OFFSET_CAL_SIGMA_TOO_HIGH,
		VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH,
		status);

	IGNORE_STATUS(
		IGNORE_OFFSET_CAL_RATE_TOO_HIGH,
		VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH,
		status);

	IGNORE_STATUS(
		IGNORE_OFFSET_CAL_SPAD_COUNT_TOO_LOW,
		VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW,
		status);

#ifdef VL53L1_LOG_ENABLE



	VL53L1_print_customer_nvm_managed(
		&(pdev->customer),
		"run_offset_calibration():pdev->lldata.customer.",
		VL53L1_TRACE_MODULE_OFFSET_DATA);

	VL53L1_print_dmax_calibration_data(
		&(pdev->fmt_dmax_cal),
		"run_offset_calibration():pdev->lldata.fmt_dmax_cal.",
		VL53L1_TRACE_MODULE_OFFSET_DATA);

	VL53L1_print_dmax_calibration_data(
		&(pdev->cust_dmax_cal),
		"run_offset_calibration():pdev->lldata.cust_dmax_cal.",
		VL53L1_TRACE_MODULE_OFFSET_DATA);

	VL53L1_print_additional_offset_cal_data(
		&(pdev->add_off_cal_data),
		"run_offset_calibration():pdev->lldata.add_off_cal_data.",
		VL53L1_TRACE_MODULE_OFFSET_DATA);

	VL53L1_print_offset_range_results(
		&(pdev->offset_results),
		"run_offset_calibration():pdev->lldata.offset_results.",
		VL53L1_TRACE_MODULE_OFFSET_DATA);
#endif

	LOG_FUNCTION_END(status);

	return status;
}


VL53L1_Error VL53L1_run_phasecal_average(
	VL53L1_DEV	            Dev,
	uint8_t                 measurement_mode,
	uint8_t                 phasecal_result__vcsel_start,
	uint16_t                phasecal_num_of_samples,
	VL53L1_range_results_t *prange_results,
	uint16_t               *pphasecal_result__reference_phase,
	uint16_t               *pzero_distance_phase)
{


	VL53L1_Error status        = VL53L1_ERROR_NONE;
	VL53L1_LLDriverData_t *pdev =
		VL53L1DevStructGetLLDriverHandle(Dev);

	uint16_t  i                                = 0;
	uint16_t  m                                = 0;
	uint32_t  samples                          = 0;

	uint32_t  period                           = 0;
	uint32_t  VL53L1_p_017                            = 0;
	uint32_t  phasecal_result__reference_phase = 0;
	uint32_t  zero_distance_phase              = 0;


	VL53L1_load_patch(Dev);

	for (m = 0; m < phasecal_num_of_samples; m++) {



		if (status == VL53L1_ERROR_NONE)
			status =
			VL53L1_init_and_start_range(
				Dev,
				measurement_mode,
				VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS);

		for (i = 0; i <= 1; i++) {



			if (status == VL53L1_ERROR_NONE)
				status =
					VL53L1_wait_for_range_completion(Dev);



			if (status == VL53L1_ERROR_NONE)
				status =
					VL53L1_get_device_results(
						Dev,
						VL53L1_DEVICERESULTSLEVEL_FULL,
						prange_results);



			if (status == VL53L1_ERROR_NONE)
				status =
					VL53L1_wait_for_firmware_ready(Dev);



			if (status == VL53L1_ERROR_NONE)
				status =
				VL53L1_clear_interrupt_and_enable_next_range(
					Dev,
					measurement_mode);
		}



		if (status == VL53L1_ERROR_NONE)
			status = VL53L1_stop_range(Dev);



		if (status == VL53L1_ERROR_NONE)
			status = VL53L1_WaitUs(Dev, 1000);



		if (status == VL53L1_ERROR_NONE) {

			samples++;


			period = 2048 *
				(uint32_t)VL53L1_decode_vcsel_period(
					pdev->hist_data.VL53L1_p_009);

			VL53L1_p_017  = period;
			VL53L1_p_017 += (uint32_t)(
			pdev->hist_data.phasecal_result__reference_phase);
			VL53L1_p_017 +=
				(2048 *
				(uint32_t)phasecal_result__vcsel_start);
			VL53L1_p_017 -= (2048 *
			(uint32_t)pdev->hist_data.cal_config__vcsel_start);

			if (period != 0) {
				VL53L1_p_017  = VL53L1_p_017 % period;
			}
			else {
				status =
				VL53L1_ERROR_DIVISION_BY_ZERO;
				VL53L1_p_017 = 0;
			}

			phasecal_result__reference_phase += (uint32_t)(
			pdev->hist_data.phasecal_result__reference_phase);

			zero_distance_phase += (uint32_t)VL53L1_p_017;
		}
	}
	VL53L1_unload_patch(Dev);



	if (status == VL53L1_ERROR_NONE && samples > 0) {

		phasecal_result__reference_phase += (samples >> 1);
		phasecal_result__reference_phase /= samples;

		zero_distance_phase += (samples >> 1);
		zero_distance_phase /= samples;

		*pphasecal_result__reference_phase =
			(uint16_t)phasecal_result__reference_phase;
		*pzero_distance_phase =
			(uint16_t)zero_distance_phase;
	}

	return status;
}


VL53L1_Error VL53L1_run_zone_calibration(
	VL53L1_DEV	                  Dev,
	VL53L1_DevicePresetModes      device_preset_mode,
	VL53L1_DeviceZonePreset       zone_preset,
	VL53L1_zone_config_t         *pzone_cfg,
	int16_t                       cal_distance_mm,
	uint16_t                      cal_reflectance_pc,
	VL53L1_Error                 *pcal_status)
{


	VL53L1_Error status        = VL53L1_ERROR_NONE;
	VL53L1_LLDriverData_t *pdev =
		VL53L1DevStructGetLLDriverHandle(Dev);

	VL53L1_LLDriverResults_t *pres =
		VL53L1DevStructGetLLResultsHandle(Dev);

	VL53L1_range_results_t         *pRR =
			(VL53L1_range_results_t *) pdev->wArea1;
	VL53L1_range_data_t            *prange_data = NULL;
	VL53L1_zone_calibration_data_t *pzone_data  = NULL;

	uint16_t  i                      = 0;
	uint16_t  m                      = 0;

	uint8_t   z                      = 0;
	uint8_t   measurement_mode       =
		VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK;

	VL53L1_OffsetCorrectionMode  offset_cor_mode =
			VL53L1_OFFSETCORRECTIONMODE__NONE;

	LOG_FUNCTION_START("");



	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_set_preset_mode(
			Dev,
			device_preset_mode,

			pdev->zonecal_cfg.dss_config__target_total_rate_mcps,
			pdev->zonecal_cfg.phasecal_config_timeout_us,
			pdev->zonecal_cfg.mm_config_timeout_us,
			pdev->zonecal_cfg.range_config_timeout_us,

			100);



	if (zone_preset == VL53L1_DEVICEZONEPRESET_CUSTOM) {

		if (status == VL53L1_ERROR_NONE)
			status =
				VL53L1_set_zone_config(
					Dev,
					pzone_cfg);

	} else if (zone_preset != VL53L1_DEVICEZONEPRESET_NONE) {

		if (status == VL53L1_ERROR_NONE)
			status =
				VL53L1_set_zone_preset(
					Dev,
					zone_preset);
	}



	pres->zone_cal.preset_mode        = device_preset_mode;
	pres->zone_cal.zone_preset        = zone_preset;

	pres->zone_cal.cal_distance_mm    = cal_distance_mm * 16;
	pres->zone_cal.cal_reflectance_pc = cal_reflectance_pc;
	pres->zone_cal.max_zones          = VL53L1_MAX_USER_ZONES;
	pres->zone_cal.active_zones       = pdev->zone_cfg.active_zones + 1;

	for (i = 0; i < VL53L1_MAX_USER_ZONES; i++) {
		pres->zone_cal.VL53L1_p_002[i].no_of_samples   = 0;
		pres->zone_cal.VL53L1_p_002[i].effective_spads = 0;
		pres->zone_cal.VL53L1_p_002[i].peak_rate_mcps  = 0;
		pres->zone_cal.VL53L1_p_002[i].VL53L1_p_014    = 0;
		pres->zone_cal.VL53L1_p_002[i].VL53L1_p_005        = 0;
		pres->zone_cal.VL53L1_p_002[i].median_range_mm = 0;
		pres->zone_cal.VL53L1_p_002[i].range_mm_offset = 0;
	}

	pres->zone_cal.phasecal_result__reference_phase = 0;
	pres->zone_cal.zero_distance_phase              = 0;



	status =
		VL53L1_get_offset_correction_mode(
			Dev,
			&offset_cor_mode);

	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_set_offset_correction_mode(
				Dev,
				VL53L1_OFFSETCORRECTIONMODE__NONE);


	VL53L1_load_patch(Dev);

	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_init_and_start_range(
				Dev,
				measurement_mode,
				VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS);




	m = (pdev->zonecal_cfg.zone_num_of_samples + 2) *
			(uint16_t)pres->zone_cal.active_zones;


	for (i = 0; i <= m; i++) {



		if (status == VL53L1_ERROR_NONE)
			status =
				VL53L1_wait_for_range_completion(Dev);



		if (status == VL53L1_ERROR_NONE)
			status =
				VL53L1_get_device_results(
					Dev,
					VL53L1_DEVICERESULTSLEVEL_FULL,
					pRR);



		prange_data  = &(pRR->VL53L1_p_002[0]);

		if (pRR->active_results > 0 &&
			i > (uint16_t)pres->zone_cal.active_zones) {

			if (prange_data->range_status ==
				VL53L1_DEVICEERROR_RANGECOMPLETE) {

				pres->zone_cal.phasecal_result__reference_phase
				=
				pdev->hist_data.phasecal_result__reference_phase
				;
				pres->zone_cal.zero_distance_phase =
					pdev->hist_data.zero_distance_phase;

				pzone_data =
				&(pres->zone_cal.VL53L1_p_002[pRR->zone_id]);
				pzone_data->no_of_samples++;
				pzone_data->effective_spads +=
				(uint32_t)prange_data->VL53L1_p_006;
				pzone_data->peak_rate_mcps  += (uint32_t)(
				prange_data->peak_signal_count_rate_mcps);
				pzone_data->VL53L1_p_014  +=
				(uint32_t)prange_data->VL53L1_p_014;
				pzone_data->VL53L1_p_005        +=
				(uint32_t)prange_data->VL53L1_p_005;
				pzone_data->median_range_mm +=
				(int32_t)prange_data->median_range_mm;

			}
		}



		if (status == VL53L1_ERROR_NONE)
			status =
				VL53L1_wait_for_firmware_ready(Dev);



		if (status == VL53L1_ERROR_NONE)
			status =
				VL53L1_clear_interrupt_and_enable_next_range(
					Dev,
					measurement_mode);
	}



	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_stop_range(Dev);


	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_WaitUs(Dev, 1000);
	VL53L1_unload_patch(Dev);


	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_run_phasecal_average(
			Dev,
			measurement_mode,
			pdev->hist_data.phasecal_result__vcsel_start,

			pdev->zonecal_cfg.phasecal_num_of_samples,

			pRR,
			&(pres->zone_cal.phasecal_result__reference_phase),
			&(pres->zone_cal.zero_distance_phase));



	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_set_offset_correction_mode(
				Dev,
				offset_cor_mode);



	if (status == VL53L1_ERROR_NONE) {

		for (z = 0; z < pres->zone_cal.active_zones; z++) {

			pzone_data = &(pres->zone_cal.VL53L1_p_002[z]);


			if (pzone_data->no_of_samples > 0) {

				pzone_data->effective_spads +=
					(pzone_data->no_of_samples/2);
				pzone_data->effective_spads /=
					pzone_data->no_of_samples;

				pzone_data->peak_rate_mcps  +=
					(pzone_data->no_of_samples/2);
				pzone_data->peak_rate_mcps  /=
					pzone_data->no_of_samples;

				pzone_data->VL53L1_p_014    +=
					(pzone_data->no_of_samples/2);
				pzone_data->VL53L1_p_014    /=
					pzone_data->no_of_samples;

				pzone_data->VL53L1_p_005        +=
					(pzone_data->no_of_samples/2);
				pzone_data->VL53L1_p_005        /=
					pzone_data->no_of_samples;



				pzone_data->median_range_mm =
				VL53L1_range_maths(
				pdev->stat_nvm.osc_measured__fast_osc__frequency
				, (uint16_t)pzone_data->VL53L1_p_014,
				pres->zone_cal.zero_distance_phase,
				2,
				0x0800,
				0);

				pzone_data->range_mm_offset  =
						((int32_t)cal_distance_mm) * 4;
				pzone_data->range_mm_offset -=
						pzone_data->median_range_mm;


				if (pzone_data->no_of_samples <
					pdev->zonecal_cfg.zone_num_of_samples)
					status =
					VL53L1_WARNING_ZONE_CAL_MISSING_SAMPLES;


				if (pzone_data->VL53L1_p_005 >
					((uint32_t)VL53L1_ZONE_CAL_MAX_SIGMA_MM
							<< 5))
					status =
					VL53L1_WARNING_ZONE_CAL_SIGMA_TOO_HIGH;

				if (pzone_data->peak_rate_mcps >
					VL53L1_ZONE_CAL_MAX_PRE_PEAK_RATE_MCPS)
					status =
					VL53L1_WARNING_ZONE_CAL_RATE_TOO_HIGH;

			} else {
				status = VL53L1_ERROR_ZONE_CAL_NO_SAMPLE_FAIL;
			}
		}
	}



	pres->zone_cal.cal_status = status;
	*pcal_status = pres->zone_cal.cal_status;



	IGNORE_STATUS(
		IGNORE_ZONE_CAL_MISSING_SAMPLES,
		VL53L1_WARNING_ZONE_CAL_MISSING_SAMPLES,
		status);

	IGNORE_STATUS(
		IGNORE_ZONE_CAL_SIGMA_TOO_HIGH,
		VL53L1_WARNING_ZONE_CAL_SIGMA_TOO_HIGH,
		status);

	IGNORE_STATUS(
		IGNORE_ZONE_CAL_RATE_TOO_HIGH,
		VL53L1_WARNING_ZONE_CAL_RATE_TOO_HIGH,
		status);

#ifdef VL53L1_LOG_ENABLE



	VL53L1_print_zone_calibration_results(
		&(pres->zone_cal),
		"run_zone_calibration():pdev->llresults.zone_cal.",
		VL53L1_TRACE_MODULE_OFFSET_DATA);

#endif

	LOG_FUNCTION_END(status);

	return status;
}


VL53L1_Error VL53L1_run_spad_rate_map(
	VL53L1_DEV                 Dev,
	VL53L1_DeviceTestMode      device_test_mode,
	VL53L1_DeviceSscArray      array_select,
	uint32_t                   ssc_config_timeout_us,
	VL53L1_spad_rate_data_t   *pspad_rate_data)
{



	VL53L1_Error status = VL53L1_ERROR_NONE;

	VL53L1_LLDriverData_t *pdev =
		VL53L1DevStructGetLLDriverHandle(Dev);

	LOG_FUNCTION_START("");


	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_enable_powerforce(Dev);



	if (status == VL53L1_ERROR_NONE) {
		pdev->ssc_cfg.array_select = array_select;
		pdev->ssc_cfg.timeout_us   = ssc_config_timeout_us;
		status =
		VL53L1_set_ssc_config(
			Dev,
			&(pdev->ssc_cfg),
			pdev->stat_nvm.osc_measured__fast_osc__frequency);
	}



	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_run_device_test(
				Dev,
				device_test_mode);



	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_get_spad_rate_data(
				Dev,
				pspad_rate_data);

	if (device_test_mode == VL53L1_DEVICETESTMODE_LCR_VCSEL_ON)
		pspad_rate_data->fractional_bits =  7;
	else
		pspad_rate_data->fractional_bits = 15;



	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_disable_powerforce(Dev);

#ifdef VL53L1_LOG_ENABLE


	if (status == VL53L1_ERROR_NONE) {
		VL53L1_print_spad_rate_data(
			pspad_rate_data,
			"run_spad_rate_map():",
			VL53L1_TRACE_MODULE_SPAD_RATE_MAP);
		VL53L1_print_spad_rate_map(
			pspad_rate_data,
			"run_spad_rate_map():",
			VL53L1_TRACE_MODULE_SPAD_RATE_MAP);
	}
#endif

	LOG_FUNCTION_END(status);

	return status;
}


VL53L1_Error VL53L1_run_device_test(
	VL53L1_DEV             Dev,
	VL53L1_DeviceTestMode  device_test_mode)
{


	VL53L1_Error status = VL53L1_ERROR_NONE;
	VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);

	uint8_t      comms_buffer[2];
	uint8_t      gpio_hv_mux__ctrl = 0;

	LOG_FUNCTION_START("");



	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_RdByte(
				Dev,
				VL53L1_GPIO_HV_MUX__CTRL,
				&gpio_hv_mux__ctrl);

	if (status == VL53L1_ERROR_NONE)
		pdev->stat_cfg.gpio_hv_mux__ctrl = gpio_hv_mux__ctrl;


	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_start_test(
					Dev,
					device_test_mode);


	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_wait_for_test_completion(Dev);


	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_ReadMulti(
				Dev,
				VL53L1_RESULT__RANGE_STATUS,
				comms_buffer,
				2);

	if (status == VL53L1_ERROR_NONE) {
		pdev->sys_results.result__range_status  = comms_buffer[0];
		pdev->sys_results.result__report_status = comms_buffer[1];
	}



	pdev->sys_results.result__range_status &=
		VL53L1_RANGE_STATUS__RANGE_STATUS_MASK;

	if (status == VL53L1_ERROR_NONE) {
		trace_print(
		VL53L1_TRACE_LEVEL_INFO,
		"    Device Test Complete:\n\t%-32s = %3u\n\t%-32s = %3u\n",
		"result__range_status",
		pdev->sys_results.result__range_status,
		"result__report_status",
		pdev->sys_results.result__report_status);


		if (status == VL53L1_ERROR_NONE)
			status = VL53L1_clear_interrupt(Dev);
	}



	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_start_test(
				Dev,
				0x00);

	LOG_FUNCTION_END(status);

	return status;
}


void VL53L1_hist_xtalk_extract_data_init(
	VL53L1_hist_xtalk_extract_data_t *pxtalk_data)
{


	int32_t lb = 0;

	pxtalk_data->sample_count             = 0U;
	pxtalk_data->pll_period_mm            = 0U;
	pxtalk_data->peak_duration_us_sum     = 0U;
	pxtalk_data->effective_spad_count_sum = 0U;
	pxtalk_data->zero_distance_phase_sum  = 0U;
	pxtalk_data->zero_distance_phase_avg  = 0U;
	pxtalk_data->event_scaler_sum         = 0U;
	pxtalk_data->event_scaler_avg         = 4096U;
	pxtalk_data->signal_events_sum        = 0;
	pxtalk_data->xtalk_rate_kcps_per_spad = 0U;
	pxtalk_data->VL53L1_p_015             = 0U;
	pxtalk_data->VL53L1_p_016               = 0U;
	pxtalk_data->target_start             = 0U;

	for (lb = 0; lb < VL53L1_XTALK_HISTO_BINS; lb++)
		pxtalk_data->bin_data_sums[lb] = 0;

}


VL53L1_Error VL53L1_hist_xtalk_extract_update(
	int16_t                             target_distance_mm,
	uint16_t                            target_width_oversize,
	VL53L1_histogram_bin_data_t        *phist_bins,
	VL53L1_hist_xtalk_extract_data_t   *pxtalk_data)
{


	VL53L1_Error  status = VL53L1_ERROR_NONE;

	LOG_FUNCTION_START("");

	status =
		VL53L1_hist_xtalk_extract_calc_window(
			target_distance_mm,
			target_width_oversize,
			phist_bins,
			pxtalk_data);

	if (status == VL53L1_ERROR_NONE) {
		status =
			VL53L1_hist_xtalk_extract_calc_event_sums(
				phist_bins,
				pxtalk_data);
	}

	LOG_FUNCTION_END(status);

	return status;
}


VL53L1_Error VL53L1_hist_xtalk_extract_fini(
	VL53L1_histogram_bin_data_t        *phist_bins,
	VL53L1_hist_xtalk_extract_data_t   *pxtalk_data,
	VL53L1_xtalk_calibration_results_t *pxtalk_cal,
	VL53L1_xtalk_histogram_shape_t     *pxtalk_shape)
{


	VL53L1_Error  status = VL53L1_ERROR_NONE;
	VL53L1_xtalk_calibration_results_t *pX = pxtalk_cal;

	LOG_FUNCTION_START("");

	if (pxtalk_data->sample_count > 0) {


		pxtalk_data->event_scaler_avg  = pxtalk_data->event_scaler_sum;
		pxtalk_data->event_scaler_avg +=
				(pxtalk_data->sample_count >> 1);
		pxtalk_data->event_scaler_avg /=  pxtalk_data->sample_count;



		status =
			VL53L1_hist_xtalk_extract_calc_rate_per_spad(
				pxtalk_data);



		if (status == VL53L1_ERROR_NONE) {


			pxtalk_data->zero_distance_phase_avg =
				pxtalk_data->zero_distance_phase_sum;
			pxtalk_data->zero_distance_phase_avg +=
					(pxtalk_data->sample_count >> 1);
			pxtalk_data->zero_distance_phase_avg /=
					pxtalk_data->sample_count;


			status =
				VL53L1_hist_xtalk_extract_calc_shape(
					pxtalk_data,
					pxtalk_shape);




			pxtalk_shape->phasecal_result__vcsel_start =
				phist_bins->phasecal_result__vcsel_start;
			pxtalk_shape->cal_config__vcsel_start =
				phist_bins->cal_config__vcsel_start;
			pxtalk_shape->vcsel_width =
				phist_bins->vcsel_width;
			pxtalk_shape->VL53L1_p_019 =
				phist_bins->VL53L1_p_019;
		}


		if (status == VL53L1_ERROR_NONE) {


			pX->algo__crosstalk_compensation_plane_offset_kcps =
				pxtalk_data->xtalk_rate_kcps_per_spad;
			pX->algo__crosstalk_compensation_x_plane_gradient_kcps
				= 0U;
			pX->algo__crosstalk_compensation_y_plane_gradient_kcps
				= 0U;

		}
	}

	LOG_FUNCTION_END(status);

	return status;
}


VL53L1_Error   VL53L1_run_hist_xtalk_extraction(
	VL53L1_DEV	                        Dev,
	int16_t                             cal_distance_mm,
	VL53L1_Error                       *pcal_status)
{


	#define OVERSIZE 4
	VL53L1_Error status = VL53L1_ERROR_NONE;
	VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
	VL53L1_xtalkextract_config_t *pX = &(pdev->xtalk_extract_cfg);
	VL53L1_xtalk_config_t *pC = &(pdev->xtalk_cfg);
	VL53L1_xtalk_calibration_results_t *pXC = &(pdev->xtalk_cal);



	uint8_t smudge_corr_en   = 0;
	uint8_t i                = 0;
	int8_t k = 0;
	uint8_t nbloops;
	int32_t initMergeSize = 0;
	int32_t MergeEnabled = 0;
	uint32_t deltaXtalk;
	uint32_t stepXtalk;
	uint32_t XtalkMin;
	uint32_t XtalkMax;
	uint8_t measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK;
	int8_t MaxId;
	uint8_t histo_merge_nb;
	uint8_t wait_for_accumulation;
	VL53L1_range_results_t     *prange_results =
		(VL53L1_range_results_t *) pdev->wArea1;
	uint8_t Very1stRange = 0;
	VL53L1_DevicePresetModes current_device_preset_mode;
	uint32_t inter_measurement_period_ms;
	uint16_t dss_config__target_total_rate_mcps;
	uint32_t phasecal_config_timeout_us;
	uint32_t mm_config_timeout_us;
	uint32_t range_config_timeout_us;

	LOG_FUNCTION_START("");

	current_device_preset_mode = pdev->preset_mode;
	inter_measurement_period_ms = pdev->inter_measurement_period_ms;


	if (status == VL53L1_ERROR_NONE)
		status =
			VL53L1_set_preset_mode(
				Dev,
				VL53L1_DEVICEPRESETMODE_HISTOGRAM_LONG_RANGE,
				pX->dss_config__target_total_rate_mcps,
				pX->phasecal_config_timeout_us,
				pX->mm_config_timeout_us,
				pX->range_config_timeout_us,
				100);



	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_disable_xtalk_compensation(Dev);



	smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;

	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_dynamic_xtalk_correction_disable(Dev);


	VL53L1_load_patch(Dev);

	VL53L1_get_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE_MAX_SIZE,
			&initMergeSize);
	VL53L1_get_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE,
			&MergeEnabled);
	memset(&pdev->xtalk_cal, 0,	sizeof(pdev->xtalk_cal));

	if (status == VL53L1_ERROR_NONE)
		status = VL53L1_init_and_start_range(
			Dev, measurement_mode,
			VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS);

	MaxId = pdev->tuning_parms.tp_hist_merge_max_size - 1;
	nbloops = (MergeEnabled == 0 ? 1 : 2);
	for (k = 0; k < nbloops; k++) {

		VL53L1_hist_xtalk_extract_data_init(
				&(pdev->xtalk_extract));
		VL53L1_set_tuning_parm(Dev,
				VL53L1_TUNINGPARM_HIST_MERGE_MAX_SIZE,
				k * MaxId + 1);

		for (i = 0; i <= pX->num_of_samples; i++) {
			if (status == VL53L1_ERROR_NONE)
				status = VL53L1_wait_for_range_completion(Dev);
			if (status == VL53L1_ERROR_NONE)
				status = VL53L1_get_device_results(Dev,
					VL53L1_DEVICERESULTSLEVEL_FULL,
					prange_results);
			Very1stRange =
				(pdev->ll_state.rd_device_state ==
				VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC);

			VL53L1_compute_histo_merge_nb(Dev, &histo_merge_nb);
			wait_for_accumulation = ((k != 0) &&
				(MergeEnabled) &&
				(status == VL53L1_ERROR_NONE) &&
				(histo_merge_nb <
				pdev->tuning_parms.tp_hist_merge_max_size));
			if (wait_for_accumulation)
				i = 0;
			else {
				if ((status == VL53L1_ERROR_NONE) &&
					(!Very1stRange)) {
					status =
					VL53L1_hist_xtalk_extract_update(
						cal_distance_mm,
						OVERSIZE,
						&(pdev->hist_data),
						&(pdev->xtalk_extract));
				}
			}

			if (status == VL53L1_ERROR_NONE)
				status = VL53L1_wait_for_firmware_ready(Dev);
			if (status == VL53L1_ERROR_NONE)
				status =
				VL53L1_clear_interrupt_and_enable_next_range(
					Dev, measurement_mode);
		}


		if (status == VL53L1_ERROR_NONE)
			status =
			VL53L1_hist_xtalk_extract_fini(
				&(pdev->hist_data),
				&(pdev->xtalk_extract),
				&(pdev->xtalk_cal),
				&(pdev->xtalk_shapes.xtalk_shape));
		if (status == VL53L1_ERROR_NONE) {
			pXC->algo__xtalk_cpo_HistoMerge_kcps[k * MaxId] =
			pXC->algo__crosstalk_compensation_plane_offset_kcps;
		}
	}


	VL53L1_stop_range(Dev);

	VL53L1_set_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE_MAX_SIZE,
			initMergeSize);
	VL53L1_unload_patch(Dev);


	VL53L1_get_preset_mode_timing_cfg(Dev, current_device_preset_mode,
		&dss_config__target_total_rate_mcps,
		&phasecal_config_timeout_us,
		&mm_config_timeout_us,
		&range_config_timeout_us);

	VL53L1_set_preset_mode(Dev, current_device_preset_mode,
		dss_config__target_total_rate_mcps,
		phasecal_config_timeout_us,
		mm_config_timeout_us,
		range_config_timeout_us,
		inter_measurement_period_ms);

	if (status != VL53L1_ERROR_NONE)
		status = VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL;
	else if ((MergeEnabled == 1) && (MaxId > 0)) {
		XtalkMin = pXC->algo__xtalk_cpo_HistoMerge_kcps[0];
		XtalkMax = pXC->algo__xtalk_cpo_HistoMerge_kcps[MaxId];
		pXC->algo__crosstalk_compensation_plane_offset_kcps =
				XtalkMin;
		if (XtalkMax >= XtalkMin) {
			deltaXtalk =  XtalkMax - XtalkMin;
			stepXtalk = deltaXtalk / MaxId;
			for (k = 1; k < MaxId; k++)
				pXC->algo__xtalk_cpo_HistoMerge_kcps[k] =
				XtalkMin + stepXtalk * k;
		} else
			status =
				VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL;
	}

	if (status == VL53L1_ERROR_NONE) {
		pC->algo__crosstalk_compensation_x_plane_gradient_kcps =
		pXC->algo__crosstalk_compensation_x_plane_gradient_kcps;
		pC->algo__crosstalk_compensation_y_plane_gradient_kcps =
		pXC->algo__crosstalk_compensation_y_plane_gradient_kcps;
		pC->algo__crosstalk_compensation_plane_offset_kcps =
		pXC->algo__crosstalk_compensation_plane_offset_kcps;
	}


	pdev->xtalk_results.cal_status = status;
	*pcal_status = pdev->xtalk_results.cal_status;


	status = VL53L1_enable_xtalk_compensation(Dev);
	if (smudge_corr_en == 1)
		status = VL53L1_dynamic_xtalk_correction_enable(Dev);

#ifdef VL53L1_LOG_ENABLE



	VL53L1_print_customer_nvm_managed(
		&(pdev->customer),
		"run_xtalk_extraction():pdev->lldata.customer.",
		VL53L1_TRACE_MODULE_XTALK_DATA);

	VL53L1_print_xtalk_config(
		&(pdev->xtalk_cfg),
		"run_xtalk_extraction():pdev->lldata.xtalk_cfg.",
		VL53L1_TRACE_MODULE_XTALK_DATA);

	VL53L1_print_xtalk_histogram_data(
		&(pdev->xtalk_shapes),
		"pdev->lldata.xtalk_shapes.",
		VL53L1_TRACE_MODULE_XTALK_DATA);

#endif

	LOG_FUNCTION_END(status);

	return status;
}

